{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "80e0a38d",
   "metadata": {},
   "outputs": [],
   "source": [
    "import panda_py\n",
    "from panda_py import libfranka\n",
    "\n",
    "panda = panda_py.Panda(\"172.16.0.2\")\n",
    "hand = libfranka.Gripper(\"172.16.0.2\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "5461c043",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Current Pose: [[    0.99999   0.0029489  -0.0012107     0.30708]\n",
      " [  0.0029473    -0.99999  -0.0012523 -0.00042655]\n",
      " [ -0.0012144   0.0012487          -1     0.48576]\n",
      " [          0           0           0           1]]\n"
     ]
    }
   ],
   "source": [
    "panda.move_to_start()\n",
    "\n",
    "print(\"Current Pose:\", panda.get_pose())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "4c17cafd",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "image 1/1 /home/robot1/JuliusWorkplace/Tongji_Intelligent_Systems_Lab/OBB_yolov8/captures/realsense.jpg: 384x640 None65.3ms\n",
      "Speed: 3.3ms preprocess, 65.3ms inference, 1.3ms postprocess per image at shape (1, 3, 384, 640)\n",
      "图片尺寸: width=1280, height=720\n",
      "[[1083.7225341796875, 179.95919799804688, 48.29425811767578, 137.18885803222656, 30.19766309431164]]\n",
      "相机内参: fx=910.25, fy=910.29, ppx=650.10, ppy=376.33, depth_scale=0.0010000000474974513\n",
      "----------results----------\n",
      "(1, 0.2472376107032451, -0.11195938915477066, 0.5190000246511772, 30.19766309431164, 1083.7225341796875, 179.95919799804688)\n"
     ]
    }
   ],
   "source": [
    "# Use run_inference() from local inference.py\n",
    "import sys\n",
    "\n",
    "folder = \"/home/robot1/JuliusWorkplace/Tongji_Intelligent_Systems_Lab/OBB_yolov8\"\n",
    "if folder not in sys.path:\n",
    "    sys.path.insert(0, folder)\n",
    "\n",
    "from inference import run_inference\n",
    "\n",
    "# Run inference and print structured results\n",
    "results = run_inference()\n",
    "print(\"----------results----------\")\n",
    "\n",
    "#(index, X, Y, Z, angle_deg, u, v)\n",
    "for item in results:\n",
    "    print(item)\n",
    "\n",
    "# Keep for later cells (e.g., motion planning)\n",
    "targets = results"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "461239f9",
   "metadata": {},
   "outputs": [],
   "source": [
    "theta_deg = results[0][4]  # Example: get angle from first result\n",
    "\n",
    "X= results[0][1]  # Example: get X from first result\n",
    "Y = results[0][2]  # Example: get Y from first result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "id": "6e9e37a7",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "# Define start and end positions for the Cartesian path\n",
    "state=panda.get_pose()\n",
    "\n",
    "state = panda.get_pose()              # 4x4 homogeneous matrix\n",
    "R = state[:3, :3]                     # keep current orientation matrix\n",
    "\n",
    "start_pos = np.array([state[0][3], state[1][3], state[2][3]])  # Extracting the current position from the pose\n",
    "end_pos1 = start_pos + np.array([-Y, -X, 0])  # Move 10 cm in the x direction\n",
    "\n",
    "num_steps=10\n",
    "positions = np.linspace(start_pos, end_pos1, num_steps, endpoint=True) # 前半段：只改 x,y\n",
    "\n",
    "\n",
    "\n",
    "# Move along the path with fixed orientation matrix R\n",
    "for pos in positions:\n",
    "    T = np.eye(4)\n",
    "    T[:3, :3] = R\n",
    "    T[:3, 3] = pos\n",
    "    panda.move_to_pose(T, dq_threshold=5e-4)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "id": "2dfd62ab",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Current Pose: [[    0.99671   -0.034962    0.072933     0.41227]\n",
      " [  -0.030003    -0.99722   -0.068016    -0.24094]\n",
      " [   0.075108    0.065605    -0.99501       0.478]\n",
      " [          0           0           0           1]]\n",
      "Current Pose: [[    0.88612    -0.45934    0.061468     0.41092]\n",
      " [   -0.45545    -0.88768   -0.067663     -0.2443]\n",
      " [   0.085644    0.031962    -0.99581     0.47355]\n",
      " [          0           0           0           1]]\n"
     ]
    }
   ],
   "source": [
    "cur_pose = panda.get_pose()\n",
    "print(\"Current Pose:\", cur_pose)\n",
    "pose=panda.get_pose()\n",
    "\n",
    "\n",
    "if theta_deg > 90:\n",
    "    theta=np.deg2rad(180-theta_deg) \n",
    "else:\n",
    "    theta=np.deg2rad(-theta_deg)\n",
    "\n",
    "Rz=np.array([\n",
    "    [np.cos(theta), -np.sin(theta), 0],\n",
    "    [np.sin(theta), np.cos(theta), 0],\n",
    "    [0, 0, 1]\n",
    "    ])\n",
    "\n",
    "pose[:3,:3]=Rz @ pose[:3,:3]\n",
    "\n",
    "panda.move_to_pose(pose, dq_threshold=5e-4)\n",
    "cur_pose = panda.get_pose()\n",
    "print(\"Current Pose:\", cur_pose)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "0e4315c4",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "\n",
    "# Get current pose and extract position and orientation (rotation matrix)\n",
    "state = panda.get_pose()              # 4x4 homogeneous matrix\n",
    "R = state[:3, :3]                     # keep current orientation matrix\n",
    "start_pos = np.array([state[0, 3], state[1, 3], state[2, 3]])\n",
    "\n",
    "standard_offset_angle=78.69\n",
    "\n",
    "object_angle= theta_deg\n",
    "\n",
    "offset_angle=object_angle-standard_offset_angle\n",
    "\n",
    "offset_y=0.055*np.cos(np.deg2rad(offset_angle))\n",
    "offset_x=0.055*np.sin(np.deg2rad(offset_angle))\n",
    "\n",
    "# Target z while keeping x,y the same\n",
    "end_pos2 = np.array([start_pos[0]+offset_x, start_pos[1]+offset_y, -0.005])\n",
    "\n",
    "\n",
    "# Interpolate positions from start to end (Cartesian path)\n",
    "num_steps = 10\n",
    "positions = np.linspace(start_pos, end_pos2, num_steps, endpoint=True)\n",
    "\n",
    "# Move along the path with fixed orientation matrix R\n",
    "for pos in positions:\n",
    "    T = np.eye(4)\n",
    "    T[:3, :3] = R\n",
    "    T[:3, 3] = pos\n",
    "    panda.move_to_pose(T, dq_threshold=5e-4)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "id": "cea8777b",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[     0.8975     -0.4338   -0.079337     0.36906]\n",
      " [    -0.4408    -0.88784    -0.13195    -0.20573]\n",
      " [  -0.013198      0.1534    -0.98808    0.006838]\n",
      " [          0           0           0           1]]\n"
     ]
    }
   ],
   "source": [
    "print(panda.get_pose())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "45cbe23c",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "hand.grasp(0.02,2,20)\n",
    "panda.move_to_start()\n",
    "hand.move(0.08,2)\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
